#include "pid.h"
#include <iostream>
#include <cmath>

class PIDImpl {
public:
    PIDImpl(double dt, double max, double min, double Kp, double Kd, double Ki);
    ~PIDImpl();
    double calculate(double setPoint, double processValue);

private:
    double _dt;
    double _max;
    double _min;
    double _Kp;
    double _Kd;
    double _Ki;
    double _pre_error;
    double _integral;
};

PID::PID(double dt, double max, double min, double Kp, double Kd, double Ki) {
    pimpl = new PIDImpl(dt, max, min, Kp, Kd, Ki);
}

double PID::calculate(double setPoint, double processValue) {
    return pimpl->calculate(setPoint, processValue);
}

PID::~PID() {
    delete pimpl;
}

PIDImpl::PIDImpl(double dt, double max, double min, double Kp, double Kd, double Ki) :
        _dt(dt),
        _max(max),
        _min(min),
        _Kp(Kp),
        _Kd(Kd),
        _Ki(Ki),
        _pre_error(0),
        _integral(0) {
}


/*
 - 比例（Proportional）(P)：

    计算当前偏差（设定值与实际值的差距），并乘以比例增益（Kp）。
    输出对系统的当前状态进行纠正。

- 积分（Integral)（I）：

    累积偏差随时间的总和，并乘以积分增益（Ki）。
    解决由于比例控制引入的静态误差，确保系统最终能够达到设定值。

- 微分（Derivative）（D）：

    计算偏差变化率，并乘以微分增益（Kd）。
    抑制系统的振荡，提高系统的稳定性。
*/
double PIDImpl::calculate(double setPoint, double processValue) {
    double error = setPoint - processValue;

    double Pout = _Kp * error;

    _integral += error * _dt;
    double Iout = _Ki * _integral;

    double derivative = (error - _pre_error) / _dt;
    double Dout = _Kd * derivative;

    double output = Pout + Iout + Dout;

    if (output > _max)
        output = _max;
    else if (output < _min)
        output = _min;

    _pre_error = error;

    return output;
}

PIDImpl::~PIDImpl() {
}
